/*
    Hardware wrapper for force-control.
    Includes a robot arm and a wrist-mounted ft sensor.
*/
#pragma once
// #include <hardware_interface/robot_hw.h>
#include "../../roboter/include/dynamica/core.hpp"
#include <iostream>
#include <string>
#include <chrono>
#include <cmath>
#include <windows.h>
#include <Eigen/Dense>
#include "../../common/sys_lock.hpp"
#include "./robot_interfaces.h"
#include "./ft_interfaces.h"

typedef std::chrono::high_resolution_clock Clock;
using namespace dynamica;


class ForceControlHardware {
  public:
    ForceControlHardware();
    ~ForceControlHardware();
    bool init(Clock::time_point time0, FTInterfaces *ft, RobotInterfaces *robot, dynamica::RobotSixaxisX *_xbot);
    void getPose(double *pose);
    int getWrench(double *wrench); // get the wrench in tool frame
    int getState(double *pose, double *wrench);
    void setPose(const double *pose_set);
    bool checkPose(const double *pose_set);
    
//init param of FT & robot interface
    void init_param();
    FTInterfaces *_ft;
    RobotInterfaces *_robot;
    dynamica::RobotSixaxisX *_kuka_bot;
};
